"""
计算imu陀螺仪和加速度计的零偏
"""
import numpy as np
import math
import csv
from imu import deg2rad, rad2deg


def get_zero_bias(filename: str):
    acc_raw_data = []
    gyro_raw_data = []
    acc_sum = [0, 0, 0]
    gyro_sum = [0, 0, 0]
    acc_bias = [0, 0, 0]  # 加速度计[ax, ay, az]的零偏值
    gyro_bias = [0, 0, 0]  # 陀螺仪[gx, gy, gz]的零偏值

    with open(filename) as f:
        reader = csv.reader(f)  # 创建csv文件读取器
        next(reader)  # 去除第一行的数据
        for row in reader:
            acc_raw_data += list(map(float, row[1:4]))  # 将string数组转成float数组
            gyro_raw_data += list(map(float, row[4:7]))
        f.close()
    n = int(len(acc_raw_data) / 3)
    print("总共统计了{}组数据, 得到的IMU各个轴的零偏如下:".format(n))
    for i in range(n):
        acc_sum[0] += acc_raw_data[3 * i]
        acc_sum[1] += acc_raw_data[3 * i + 1]
        acc_sum[2] += acc_raw_data[3 * i + 2]
        gyro_sum[0] += gyro_raw_data[3 * i]
        gyro_sum[1] += gyro_raw_data[3 * i + 1]
        gyro_sum[2] += gyro_raw_data[3 * i + 2]

    acc_bias[0] = acc_sum[0] / n
    acc_bias[1] = acc_sum[1] / n
    acc_bias[2] = acc_sum[2] / n

    gyro_bias[0] = gyro_sum[0] / n
    gyro_bias[1] = gyro_sum[0] / n
    gyro_bias[2] = gyro_sum[0] / n

    return acc_bias, gyro_bias


acc_zero_bias_ref = 0.9 * 0.001 * 9.8   # units[m/s^2]
gyro_zero_bias_ref = deg2rad(0.025)     # units[rad/s]

acc_bias, gyro_bias = get_zero_bias("imu_static.csv")
print("说明书上加速度计零偏的参考值为：{:.6f} m/s^2".format(acc_zero_bias_ref))
print("ax_bias = {0:.6f}\nay_bias = {1:.6f}\naz_bias = {2:.6f}".format(acc_bias[0], acc_bias[1], 9.8-acc_bias[2]))
print("说明书上陀螺仪零偏的参考值为：{:.6f} rad/s".format(gyro_zero_bias_ref))
print("gx_bias = {0:.6f}\ngy_bias = {1:.6f}\ngz_bias = {2:.6f}".format(gyro_bias[0], gyro_bias[1], gyro_bias[2]))


